#!/bin/bash
# EchokitBot micro-ROS语音控制系统启动脚本

set -e

GREEN='\033[0;32m'
YELLOW='\033[1;33m'
RED='\033[0;31m'
BLUE='\033[0;34m'
NC='\033[0m'

echo -e "${BLUE}╔══════════════════════════════════════════════════════════════╗${NC}"
echo -e "${BLUE}║       EchokitBot micro-ROS语音控制系统启动                   ║${NC}"
echo -e "${BLUE}╚══════════════════════════════════════════════════════════════╝${NC}"
echo ""

# 设置环境
source /opt/ros/jazzy/setup.bash 2>/dev/null || source /opt/ros/humble/setup.bash
export ROS_DOMAIN_ID=0  # EchokitBot使用域0
cd "$(dirname "$0")/.."
source install/setup.bash 2>/dev/null || echo "⚠️  项目未构建，请先运行 colcon build"

echo "🔧 系统架构:"
echo "   [EchoKit Box] → [EchoKit Server:8060] → [Voice Control Node] → [ROS2 Topics] → [micro-ros-agent:8888] → [EchokitBot UDP]"
echo ""

# 1. 检查micro-ros-agent (已注释 - 手动启动)
echo -e "${YELLOW}🤖 micro-ros-agent状态...${NC}"
echo "✅ 假设micro-ros-agent已在8888端口手动启动"
echo "   如需检查状态，请手动运行: docker ps | grep micro-ros-agent"

# 注释掉的Docker检查和启动代码:
# if docker ps | grep -q micro-ros-agent; then
#     echo "✅ micro-ros-agent运行正常"
#     docker ps --filter name=micro-ros-agent --format "   容器: {{.Names}} | 状态: {{.Status}}"
# else
#     echo "❌ micro-ros-agent未运行"
#     echo "启动micro-ros-agent..."
#     docker run -d \
#         --name micro-ros-agent \
#         --restart=unless-stopped \
#         --net=host \
#         microros/micro-ros-agent:jazzy \
#         udp4 --port 8888 -v6
#     sleep 3
#     echo "✅ micro-ros-agent已启动"
# fi

# 2. 检查EchoKit Server
echo ""
echo -e "${YELLOW}🎤 检查EchoKit Server状态...${NC}"

# 设置EchoKit服务器地址
ECHOKIT_IP=${ECHOKIT_SERVER_IP:-"192.168.2.35"}
ECHOKIT_PORT=${ECHOKIT_SERVER_PORT:-"8060"}

# 检查HTTP端口
if curl -s --connect-timeout 3 http://${ECHOKIT_IP}:${ECHOKIT_PORT}/health > /dev/null 2>&1; then
    echo "✅ EchoKit Server HTTP端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 运行正常"
else
    echo "⚠️  EchoKit Server HTTP端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 不可达"
fi

# 检查WebSocket端口
if timeout 3 bash -c "</dev/tcp/${ECHOKIT_IP}/${ECHOKIT_PORT}" 2>/dev/null; then
    echo "✅ EchoKit Server WebSocket端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 可连接"
else
    echo "⚠️  EchoKit Server WebSocket端口 (${ECHOKIT_IP}:${ECHOKIT_PORT}) 不可达"
    echo "   请确保EchoKit Server正在运行并监听${ECHOKIT_IP}:${ECHOKIT_PORT}端口"
fi

# 检查配置文件
if [ -f "/Users/vesperxiao/rust/echokit_server0907_robot/config.toml" ]; then
    echo "✅ EchoKit配置文件存在"
    if grep -q "\[forward\]" "/Users/vesperxiao/rust/echokit_server0907_robot/config.toml"; then
        echo "✅ 配置文件包含方括号格式命令"
    else
        echo "⚠️  配置文件可能缺少方括号格式命令"
    fi
else
    echo "⚠️  EchoKit配置文件未找到"
fi

# 3. 检查EchokitBot网络
echo ""
echo -e "${YELLOW}🤖 检查EchokitBot网络连通性...${NC}"
if ping -c 2 192.168.2.42 > /dev/null 2>&1; then
    echo "✅ EchokitBot (192.168.2.42) 网络可达"
else
    echo "⚠️  EchokitBot (192.168.2.42) 网络不可达"
fi

# 4. 启动语音控制系统
echo ""
echo -e "${YELLOW}🎯 启动micro-ROS语音控制系统...${NC}"
echo "配置:"
echo "  - EchoKit Server: ws://${ECHOKIT_SERVER_IP:-192.168.2.35}:${ECHOKIT_SERVER_PORT:-8060}/ws/robot"
echo "  - micro-ros-agent: localhost:8888 (UDP)"
echo "  - EchokitBot: 192.168.2.42 (通过micro-ROS)"
echo ""
echo "🎯 集成功能:"
echo "  ✅ EchoKit消息处理器已集成"
echo "  ✅ 支持方括号格式 [action] 和JSON格式"
echo "  ✅ WebSocket客户端完全实现"
echo "  ✅ 自动重连和错误处理"
echo "  ✅ 安全检查和紧急停止"
echo ""

# 启动系统
echo -e "${GREEN}🚀 启动EchokitBot语音控制系统...${NC}"
echo ""
echo "支持的语音命令:"
echo "  中文: 前进、后退、左转、右转、停止"
echo "  英文: forward, backward, turn left, turn right, stop"
echo "  方括号格式: [forward], [stop] 等"
echo ""
echo "系统将自动:"
echo "  ✅ 连接到EchoKit Server WebSocket"
echo "  ✅ 解析多种消息格式"
echo "  ✅ 发布cmd_vel命令到EchokitBot"
echo "  ✅ 监控连接状态和安全检查"
echo ""
echo -e "${YELLOW}按 Ctrl+C 停止系统${NC}"
echo ""

# 检查EchoKit服务器地址
ECHOKIT_SERVER_IP=${ECHOKIT_SERVER_IP:-"192.168.2.35"}
ECHOKIT_SERVER_PORT=${ECHOKIT_SERVER_PORT:-"8060"}
ECHOKIT_SERVER_URL="ws://${ECHOKIT_SERVER_IP}:${ECHOKIT_SERVER_PORT}/ws/robot"

echo -e "${GREEN}使用EchoKit服务器地址: ${ECHOKIT_SERVER_URL}${NC}"
echo ""

ros2 run echokitbot_microros_voice llm_bridge_voice_control_node \
    --ros-args \
    -p server.url:=${ECHOKIT_SERVER_URL} \
    -p server.reconnect_interval:=5.0 \
    -p movement.max_linear_velocity:=0.5 \
    -p movement.max_angular_velocity:=1.0 \
    -p safety.command_timeout:=30.0 \
    --log-level info